//
// Created by rebeater on 3/23/21.
//

/**global*/
#include <global_defines.h>

/*user*/
#include "RtTasks.h"
#include "led.h"
#include <agilex.h>
#include <psins_run.h>
#include "rtcommon.h"
#include <confgrw.h>
#include "nmea.h"
#include "navmsg.h"
#include "GnssParseRaw.h"
/** library */
#include "FreeRTOS.h"
#include "cmsis_os.h"

/**drivers*/
#include <usart.h>
#include <fatfs.h>
#include <tim.h>
#include <fdcan.h>
/** language*/
#include <stdio.h>
#include <string.h>

FDCAN_RxHeaderTypeDef RxHeader;

extern osMessageQId imuDataQueueHandle;
extern osMessageQId gnssDataQueueHandle;
extern osMessageQId rsltDataQueueForStorageHandle;/*其他任务发送到存储任务*/
extern osMessageQId velQueueHandle;
extern osMessageQId msgIcmInterruptQueueHandle;

extern osThreadId alignTaskHandle;
osThreadId blinkTaskHandle;
osThreadId timingTaskHandle;
osThreadId gnssTaskHandle;
osThreadId dataStorageTaskHandle;
osThreadId looselyCoupleTaskHandle;
osThreadId imuReadTask;
extern DateTime date_time;
extern Option default_opt;
static double uart_gpst = 0;/*gnss time 上时uart的GPS时间*/
NavOutput navOutput;
typedef enum {
  FLAG_IMU = 0x01,
  FLAG_GNSS = 0x02,
  FLAG_ODO = 0x04,
  FLAG_BARO = 0x08,
  FLAG_MAG = 0x10,
  FLAG_SLAM = 0x20,
} FlagSensors;

const uint64_t pps_margin = 1000000LL;/*两个PPS之间的间隔，用于修正本地时间 */
__IO uint64_t cnt_on_pps; /*CNT计数*/

extern uint8_t flag_camera_event_ready;

__IO FlagTiming
	flag_timing = UNTIMING;/*本地授时标志，0 未经授时 1：pps已经到达 2：授时完成*/
__IO uint32_t
	system_time_int = 0;
__IO double system_time_start = 0;/*系统时间起点，gps*/

__IO SystemState
	system_state = SYSTEM_START;
SemaphoreHandle_t uartGnssReadySemaphoreHandle;

/*buffers*/
uint8_t rx_buff[UART_RX_BUFFER_LENGTH]    ALIGN_RAM_D1;/*GNSS接收数据缓存*/
uint8_t uart1_rx_buff[UART_RX_BUFFER_LENGTH]    ALIGN_RAM_D1;/*GNSS接收数据缓存*/
uint8_t uart6_rx_buffer[UART_RX_BUFFER_LENGTH] ALIGN_RAM_D1;
uint8_t gnss_buff[UART_RX_BUFFER_LENGTH]    ALIGN_RAM_D1;
uint8_t rtcm3_buff[UART_RX_BUFFER_LENGTH]    ALIGN_RAM_D1;
extern char kUart6TxBuffer[512];

char kUart3TxBuffer[256] ALIGN_RAM_D1;/* Uart3 transmit buffer for GNRMC field*/

void SdResultHandle(int x, const char *info) {
  if (x != 0) {
//	system_state = SYSTEM_WARNING;
//	while (1) {
	trace(Error, "%d\t%s", x, info);
//	  osDelay(2000);
//	}
  }
}
void ShowWelcome() {
  char hello[]= "hello world\n";
  trace(Info,Description);
}
void ShowConfig(Option *opt) {
  static char buff[512];
  sprintf(buff,
		  "IMU rate=%d\n Initial acce bias %d %d %d mGal\n Initial gyro bias %d %d %d deg/h \n",
		  (int)opt->imu_data_rate,
		  (int)(opt->imuPara.ab_ini[0] / _mGal),
		  (int)(opt->imuPara.ab_ini[1] / _mGal),
		  (int)(opt->imuPara.ab_ini[2] / _mGal),
		  (int)(opt->imuPara.gb_ini[0] / _deg * _hour),
		  (int)(opt->imuPara.gb_ini[1] / _deg * _hour),
		  (int)(opt->imuPara.gb_ini[2] / _deg * _hour)
  );
  HAL_UART_Transmit(&huart6, (uint8_t *)buff, strlen(buff), 0xffff);
  sprintf(buff,
		  "Initial odometer scale factor: %d *e-2\n Accelerator bias std:%d  mGal\n Gyroscope bias std: %d  deg/h\n"
		  "Gnss enable:%d\n",
		  (int)(100 * opt->odo_kd),
		  (int)(opt->imuPara.ab_std / _mGal),
		  (int)(opt->imuPara.gb_std / _deg * _hour),
		  (int)opt->enable_gnss
  );
  HAL_UART_Transmit(&huart6, (uint8_t *)buff, strlen(buff), 0xffff);
}

void AlignIndoor() {
  ImuMsg imu_msg;
  ImuData imu;
  BaseType_t res;
  kalmanAlignUseDefualt();
  int r;
  do {
	res = xQueueReceive(imuDataQueueHandle, &imu_msg, portMAX_DELAY);
	if (res != pdTRUE) {
	  trace(Error, "error in AlignMovingTask reading IMU: res = %ld\n", res);
	  continue;
	}
	RAW_TO_IMU(&imu_msg.data, &imu);
	r = kalmanAlignLevel(&imu);
//	trace(Info, "imu gpst = %d\n", (int)(1000 * imu.gpst));
  } while (r != 1);

}

_Noreturn void MainTask() {
  system_state = SYSTEM_START;
  LedBlinkTest();
  ShowWelcome();
  FRESULT res = f_mount(&SDFatFS, SDPath, 1);
  if (res == FR_OK) {
	osThreadDef(dataStorageTask, SDCardStorageTask, osPriorityLow, 0, 512 * 3 + 128);
	dataStorageTaskHandle = osThreadCreate(osThread(dataStorageTask), NULL);
	Option temp;
	res = LoadConfig(".initial", &temp);
	if (res != 0) {
	  trace(Error, "config file load failed(%d)\n", res);
	} else {
	  *(&default_opt) = temp;
	  ShowConfig(&default_opt);
	}
  } else {
	/*SD卡挂载失败，堵塞*/
	trace(Fetal, "SDcard mount failed(%d),Please check whether TF card is inserted\n", res);
  }
  osThreadDef(BlinkTask, BlinkTask, osPriorityNormal, 0, 1024);
  blinkTaskHandle = osThreadCreate(osThread(BlinkTask), NULL);
    trace(Info, "init IMU device:%s\n", ImuName);
#if 0
  trace(Info,
		"size of RAW_Data = %d Velocity = %d NavOutput = %d GnssData = %d IMU_RAW = %d\n",
		sizeof(RawData),
		sizeof(Velocity),
		sizeof(NavOutput),
		sizeof(GnssData),
		sizeof(IMU_RAW)
		);
#endif
  osDelay(100);
  INIT_IMU_DEVICE();
  uartGnssReadySemaphoreHandle = xSemaphoreCreateBinary();
  trace(Info, "\t\t\t\tOK\n");
  osThreadDef(imuReadTask, ImuReadSpiTask, osPriorityNormal, 0, 512);
  imuReadTask = osThreadCreate(osThread(imuReadTask), NULL);
  system_state = SYSTEM_WAIT_FOR_TIME;
  if (default_opt.enable_gnss) {
	trace(Info, "detect GNSS receiver......\n");
#if   GNSS_RCV == GNSS_RCV_UB482
	__HAL_UART_ENABLE_IT(&huart5, UART_IT_IDLE);
	HAL_UART_Receive_DMA(&huart5, rx_buff, UART_RX_BUFFER_LENGTH);
#elif GNSS_RCV == GNSS_RCV_MXT906B
	__HAL_UART_ENABLE_IT(&huart2, UART_IT_IDLE);
	HAL_UART_Receive_DMA(&huart2, rx_buff, UART_RX_BUFFER_LENGTH);
#endif
	trace(Info, "wait for TIME...\n");
	LocalTimingTask();
	trace(Info, "current time is %d\nstart aligning\n", (int)system_time_start);
	if (kalmanInitialize()) {
	  trace(Error, "memory malloc failed\n");
	  Error_Handler();
	}
	AlignTask();
  } else {
	/*无GPS模式启动*/
	gpst_start_of_day = 18;
	system_time_start = 0;
	system_time_int = 0;
	gps_current_week = 2187;
	gpst_to_utc(gps_current_week, system_time_start, &date_time);
	flag_timing = TIMING_OK;
	system_state = SYSTEM_ALIGNING;
	trace(Info, "start aligning\n");
	HAL_TIM_Base_Start_IT(&htim2);
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
	AlignIndoor();
  }
/*  __HsudAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);
  HAL_UART_Receive_DMA(&huart1, uart1_rx_buff, UART_RX_BUFFER_LENGTH);*/
  __HAL_UART_ENABLE_IT(&huart6, UART_IT_IDLE);
  HAL_UART_Receive_DMA(&huart6, uart6_rx_buffer, UART_RX_BUFFER_LENGTH);
  system_state = SYSTEM_NAVIGATION;
  trace(Info, "start loosely couple\n");
  osThreadDef(looselyCouple, LooselyCoupleMainTask, osPriorityHigh, 0, 4096);
  looselyCoupleTaskHandle = osThreadCreate(osThread(looselyCouple), NULL);
  trace(Info, "start output\n");
  /*打开里程计*/
  HAL_FDCAN_Start(&hfdcan1);
  for (;;) {
	/*	unsigned portBASE_TYPE res1, res2, res3, res4;
		res1 = uxTaskGetStackHighWaterMark(looselyCoupleTaskHandle);
		res2 = uxTaskGetStackHighWaterMark(output_task_handle);
		res3 = uxTaskGetStackHighWaterMark(dataStorageTaskHandle);
		res4 = uxTaskGetStackHighWaterMark(start_task_handle
		 trace(Info, "%02d:%02d:%02d:  %lu/%d %lu/%d %lu/%d %lu/%d\n", date_time.hour, date_time.minute,
			   date_time.second, res1,
			   4096, res2, 1024, res3, 1024, res4, 1024);*/
	osDelay(5000);
  }
}

/*本地授时任务，完成任务之后删除*/
void LocalTimingTask() {
  GnssMsg gnss_buf;
  BaseType_t res;
  while (1) {
	if (flag_timing != TIMING_OK) {
	  res = xQueueReceive(gnssDataQueueHandle, &gnss_buf, portMAX_DELAY);
	  if (res != pdTRUE && (gnss_buf.week > 0)) {
		printf("error in LocalTimingTask res = %ld\n", res);
	  } else {
		uart_gpst = gnss_buf.gpst;
		flag_timing = WAIT_FOR_PPS;
		gpst_to_utc(gnss_buf.week, gnss_buf.gpst, &date_time);
	  }
	} else {
	  break;
	}
	osDelay(1);
  }
  // 授时完成，开始采集数据和对准
}
__IO uint32_t valid_length;
/*GNSS NMEA解析任务，将数据放入队列，，但是占用也不高*/
_Noreturn void GnssTask() {
  BaseType_t res;
  FRESULT fresult;
  GnssMsg gnss;
  uint32_t data_length = 0;
  UINT len;
  char range_path[128] = "hello world";
  uint8_t flag_raw_file_ready;
  while (1) {
	if (xSemaphoreTake(uartGnssReadySemaphoreHandle, portMAX_DELAY) == pdTRUE) {
	  valid_length = PARSE_GNSS((gnss_buff), &gnss);
	  if (valid_length > 0) {
#if DEBUG_INDOOR == 1
		trace(Info,
			  "GNSS: %d MODE: %d YAW: %d Ns:%d\n",
			  (int)gnss.gpst,
			  gnss.data.mode,
			  (int)gnss.data.yaw,
			  gnss.data.ns);
#endif
		res = xQueueSend(gnssDataQueueHandle, &gnss, 0xf);
//		res = xQueueSend(gnssDataQueueForStorageHandle, &gnss, 0xf);
		/* 保存原始数据*/
	  }
	}
  }
}

/*现在是有返回值，因为作为startDefaultTask的一部分*/
void AlignTask() {
//  trace(Info, "AlignTask\n");
  double vel = 0;
  ImuMsg imu_msg;
  BaseType_t res;
  GnssData gnss;
  ImuData imu;
  BaseType_t pxHigherPriorityTaskWoken;
  while (pdTRUE) {
	res = xQueueReceive(imuDataQueueHandle, &imu_msg, 500);
	if (res != pdTRUE) {
	  trace(Error, "error in AlignMovingTask reading IMU: res = %ld\n", res);
	  continue;
	}
	RAW_TO_IMU(&imu_msg.data, &imu);
	kalmanAlignLevel(&imu);
	res = xQueueReceive(gnssDataQueueHandle, &gnss, 0);
#if ENABLE_DOUBLE_ANTENNA == 1
	if (res == pdTRUE && isValid(&gnss)) {
	  /*双天线对准*/
	  if (kalmanAlignYawByDoubleAntenna(&gnss, &imu) > 0) {
		trace(Info, "%d:Double Antenna Align finished\n", (int)gnss.gpst);
		break;
	  } else {
		trace(Info, "%d:Heading information captured: %d\n", (int)gnss.gpst, (int)gnss.yaw);
	  }
	}
#else
	if (res == pdTRUE && isValid(&gnss)) {
	  /*动对准*/
	  vel = kalmanAlignPos(&gnss, &imu);
	  trace(Info, "Moving Align:vel = %d mm/s\n", (int)(1000 * vel));
	}
	if (kalmanAlignLevel(&imu)) {
	  kalmanOutput(&navOutput);
	  trace(Info, "align finished heading = %d mdeg\n", (int)(1000 * navOutput.atti[2] / _deg));
	  break;
	}
#endif
  }
  kalmanOutput(&navOutput);
  trace(Info,
		"Initial Attitude: %d %d %d\n",
		(int)(navOutput.atti[0] / _deg),
		(int)(navOutput.atti[1] / _deg),
		(int)(navOutput.atti[2] / _deg));
}
GnssMode mode;
uint8_t out_buffer[128] __attribute__((section(".Uart4TxSection")));
inline void SendIMUToSdTask(ImuMsg *imu) {
  RawDataDef rawdata;
  rawdata.imu_.raw_ = imu->data;
  rawdata.type_ = DATA_TYPE_IMU;
  rawdata.gpst = imu->gpst;
  GetBias(rawdata.imu_.gb_, rawdata.imu_.ab_);
  xQueueSend(rsltDataQueueForStorageHandle, &rawdata, 0);
}
inline void SendGnssToSdTask(GnssMsg *gnss_msg) {
  RawDataDef rawdata;
  rawdata.gnss_ = gnss_msg->data;
  rawdata.type_ = DATA_TYPE_GNSS;
  rawdata.gpst = gnss_msg->gpst;
  xQueueSend(rsltDataQueueForStorageHandle, &rawdata, 0);
}
inline void SendRstToSdTask(double gpst, NavOutput *nav) {
  RawDataDef rawdata;
  rawdata.rst_ = *nav;
  rawdata.type_ = DATA_TYPE_RST;
  rawdata.gpst = gpst;
  xQueueSend(rsltDataQueueForStorageHandle, &rawdata, 0);
}
inline void SendVelToSdTask(double gpst, VelocityMsg *vel) {
  RawDataDef rawdata;
  rawdata.vel_ = vel->data;
  rawdata.type_ = DATA_TYPE_VEL;
  rawdata.gpst = vel->gpst;
  xQueueSend(rsltDataQueueForStorageHandle, &rawdata, 0);
}
_Noreturn void LooselyCoupleMainTask() {
  trace(Info, "LooselyCoupleMainTask\n");
  ImuMsg imu_msg = {{}, 0};
  GnssMsg gnss_msg = {{}, 0, 0};
  VelocityMsg vel_msg = {{}, 0};
  ImuData imu;
  Velocity vel;
  BaseType_t res;
  uint32_t flag_sensors = 0x00;
  double next_gpst = 0;
  kalmanInitialize();
  while (pdTRUE) {
	res = xQueueReceive(imuDataQueueHandle, &imu_msg, 0xff);
	if (res != pdTRUE) {
	  continue;
	}
	SendIMUToSdTask(&imu_msg);
	flag_sensors |= FLAG_IMU;
	res = xQueueReceive(gnssDataQueueHandle, &gnss_msg, 0);
	if (res == pdTRUE) {
#if DEBUG_INDOOR == 1
	  trace(Info,
			"GNSS: %d MODE: %d YAW: %d Ns:%d dt= %4d ms\n",
			(int)gnss_msg.gpst,
			gnss_msg.data.mode,
			(int)gnss_msg.data.yaw,
			gnss_msg.data.ns,
			(int)(1000 * (gnss_msg.gpst - imu_msg.gpst))
	  );
#endif
	  SendGnssToSdTask(&gnss_msg);
	  mode = gnss_msg.data.mode;
	  flag_sensors |= FLAG_GNSS;
	  if (isValid(&gnss_msg.data) > 0) {
		kalmanSetGNSS(&gnss_msg.data, imu_msg.gpst - gnss_msg.gpst);
	  } else {
		mode = INVALID;
	  }
	}
	res = xQueueReceive(velQueueHandle, &vel_msg, 0);
	if (res == pdTRUE) {
	  flag_sensors |= FLAG_ODO;
	  ConvertVelRawToFloat(&vel_msg.data, &vel);
	  kalmanSetVel(vel.forward);
	  SendVelToSdTask(imu_msg.gpst, &vel_msg);
	}
	RAW_TO_IMU(&imu_msg.data, &imu);
	kalmanUpdate(&imu);
	kalmanOutput(&navOutput);
	navOutput.info.mode = mode;
	navOutput.info.sensors = flag_sensors;
	SendRstToSdTask(imu_msg.gpst, &navOutput);
	/*! 主输出端口，输出高频组合导航消息，建议采用DMA,打开一路超高速串口*/
	size_t sz = AgilexEncode(out_buffer, gps_current_week, imu_msg.gpst, &navOutput);
	SCB_CleanInvalidateDCache_by_Addr((uint32_t *)out_buffer, 96);
	HAL_UART_Transmit_DMA(&huart4, out_buffer, sz);
	if (imu_msg.gpst >= next_gpst) { /*1Hz*/
	  next_gpst = (int)imu_msg.gpst + 1.0;
	  sprintOutput(kUart6TxBuffer, gps_current_week, imu_msg.gpst, &navOutput);
	  SCB_CleanInvalidateDCache_by_Addr((uint32_t *)kUart6TxBuffer, 512);
	  HAL_UART_Transmit_DMA(&huart6, (uint8_t *)kUart6TxBuffer, strlen(kUart6TxBuffer));
	  /*GNRMC Output*/
//	  trace(Info,"imu gpst:%d\n",(int)(1000*(imu.gpst)));
	  int len = Nav2GnRmc(imu_msg.gpst, &navOutput, kUart3TxBuffer);
	  if (len > 0) {
		SCB_CleanInvalidateDCache_by_Addr((uint32_t *)kUart3TxBuffer, 256);
		HAL_UART_Transmit_DMA(&huart3, (uint8_t *)kUart3TxBuffer, len);
	  }
	}
  }
}
RawDataDef raws[64] ALIGN_RAM_D1/*__attribute__((section(".RstBuffer1024"))) */ ;
_Noreturn void SDCardStorageTask() {
  int idx_imu = 0, idx_rslt = 0, idx_vel = 0, idx_ent = 0, raw_length = 0;
  uint8_t idx_gnss = 0;
  FRESULT res, res1, res2;
  char rslt_filename[25], event_filename[25], graw_filename[25];
  /*! d等待授时完成之后开始写入数据*/
  while (flag_timing != TIMING_OK) { osDelay(10); }
  double gpst = GetCurrentGpst();
  trace(Info, "current time:%d\n", (int)gpst);
  if (!default_opt.enable_gnss) {
	/*禁用GNSS的花*/
	int file_index_1 = 1, file_index_2 = 1, file_index_3 = 1, file_index_4 = 1, file_index_5 = 1;
	FRESULT res3, res4;
	do {
	  sprintf(rslt_filename, "RAW_INDOOR%06d.raw", file_index_3++);
	  res3 = f_open(&RsltFile, rslt_filename, FA_READ);
	  f_close(&RsltFile);
	} while (res3 == FR_OK);

  } else {
	/*启用GNSS*/
	sprintf(rslt_filename, "RAW_%02d%02d%02d_%06d.RAW", date_time.year - 20, date_time.mon, date_time.day, (int)gpst);
	sprintf(graw_filename, "GRAW_%02d%02d%02d_%06d.graw", date_time.year - 20, date_time.mon, date_time.day, (int)gpst);
	res = f_open(&GNSSRawFile, graw_filename, FA_CREATE_ALWAYS | FA_WRITE);
	SdResultHandle(res, "gnss raw open failed\n");
  }
  UINT len;
  res = f_open(&RsltFile, rslt_filename, FA_CREATE_ALWAYS | FA_WRITE);
  SdResultHandle(res, "rslt open failed\n");
  trace(Info, "Create file %s\n", rslt_filename);
  /* Infinite loop */
  for (;;) {
	/* Info */
	if (default_opt.enable_gnss && xSemaphoreTake(uartGnssReadySemaphoreHandle, 0) == pdTRUE) {
	  res = f_write(&GNSSRawFile, gnss_buff, valid_length, &len);
	  raw_length += valid_length;
	  if (raw_length > 4096) {
		f_sync(&GNSSRawFile);
		raw_length = 0;
	  }
	  valid_length = 0;
	}
	if (idx_rslt >= 64) {
	  idx_rslt = 0;
	  res1 = f_write(&RsltFile, raws, sizeof(RawDataDef) * 64, &len);
	  res2 = f_sync(&RsltFile);
	  SdResultHandle(res1, "result write failed\n");
	  SdResultHandle(res2, "result sync failed\n");
	}//*/

#if ENABLE_CAMERA == 1
	if (idx_ent >= 32) {
	  idx_ent = 0;
	  res1 = f_write(&EntFile, events, sizeof(CameraEvent) * 32, &len);
	  res2 = f_sync(&EntFile);
	  if (res1 == FR_OK && res2 == FR_OK) {
		trace(Info, "write event file %d bytes\n", len);
	  } else {
		trace(Error, "write EntFile failed res1 = %d res2 = %d\n", res1, res2);
		flag_error = 1;
		ErrorInStorage();
	  }
	}//*/
#endif
	res = xQueueReceive(rsltDataQueueForStorageHandle, &raws[idx_rslt], 0);
	if (res == pdTRUE) {
	  idx_rslt++;
	}//*/
  }
}

/**
 * LED 任务 闪灯控制
 * @param c 颜色
 * @param up 亮灯时间
 * @param down 灭灯时间
 */
void Blink(LedColor c, int up, int down) {
  SetLed(c);
  osDelay(up);
  SetLed(LED_OFF);
  osDelay(down);
}
//#if IMU_DEVICE == IMU_ICM20602
_Noreturn void ImuReadSpiTask() {
  BaseType_t res1, res2;
  ImuMsg raw;
  ImuData imu;
  uint8_t state;
  while (1) {
	res1 = xQueueReceive(msgIcmInterruptQueueHandle, &raw, 0xff);
	if (res1 == pdTRUE) {
	  READ_IMU_FIFO(&raw.data, raw.id);
#if ENABLE_MULTI_IMUS == 1
	  if (raw.id == 1) {
		int temp = (int)(25 + raw.temp / 326.8);
		if (temp < 45 && temp > 20)
//		trace(Info, "%2d %5d %6d %3d\n", raw.id, temp, (int)(imu.acce[2] * 1000), length);
		  res1 = xQueueSend(imuDataQueueHandle, &raw, 0xff);
		osDelay(1);
	  }
#else
	  res1 = xQueueSend(imuDataQueueHandle, &raw, 0xff);
#endif
//	  res2 = xQueueSend(imuDataQueueForStorageHandle, &raw, 0xff);
	  if (res1 != pdTRUE) {
		trace(Info, "IMU queue is FULL %ld %ld\n", res1, res1);
		osDelay(10);
	  }
	} else {
	  osDelay(1);
	}
  }
}
//#endif
_Noreturn void BlinkTask() {
  while (1) {
	switch (system_state) {
	  case SYSTEM_START:SetLed(LED_RED);
		osDelay(1);
		break;
	  case SYSTEM_WAIT_FOR_TIME:Blink(LED_RED, 500, 500);/*修改为红色增加区分度*/
		break;
	  case SYSTEM_ALIGNING:Blink(LED_YELLOW, 200, 300);
		break;
	  case SYSTEM_NAVIGATION:
		switch (mode) {
		  case RTK_DGPS:SetLed(LED_BLUE);
			break;
		  case RTK_FLOAT:SetLed(LED_BLUE);
			break;
		  case RTK_FIX:SetLed(LED_GREEN);
			break;
		  case SPP:SetLed(LED_CYAN);
			break;
		  case INITIAL:
		  case UNKNOWN_PPS:
		  case INVALID:SetLed(LED_PINK);
			break;
		  default:SetLed(LED_PINK);
			break;
		}
		osDelay(400);
		SetLed(LED_OFF);
		osDelay(1600);
		break;
	  case SYSTEM_WARNING: { Blink(LED_RED, 300, 700); }
		break;
	  case SYSTEM_ERROR: while (1) { Blink(LED_WHITE, 100, 100); }
		break;
	  default:break;
	}
  }
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
#if IMU_DEVICE == IMU_ICM20602
  BaseType_t res, px_higher_priority_task_woken;
  IMU_RAW raw;
//trace(Info,"%X %X\n",GPIO_Pin,(ICM_INT_0_Pin | ICM_INT_1_Pin | ICM_INT_2_Pin | ICM_INT_3_Pin));
  if ((GPIO_Pin & (ICM_INT_0_Pin | ICM_INT_1_Pin | ICM_INT_2_Pin | ICM_INT_3_Pin))) {
	if (flag_timing != TIMING_OK) {
	  return;
	}
#if  ENABLE_MULTI_IMUS == 1
	switch (GPIO_Pin) {
	  case ICM_INT_0_Pin: icm20602_id = 0;
		break;
	  case ICM_INT_1_Pin: icm20602_id = 1;
		break;
	  case ICM_INT_2_Pin: icm20602_id = 2;
		break;
	  case ICM_INT_3_Pin: icm20602_id = 3;
		break;
	}
#endif
	raw.id = icm20602_id;
	raw.PIN_ID = GPIO_Pin;
	raw.gpst = system_time_start + (system_time_int) + htim2.Instance->CNT * 1.0 / pps_margin;
	res = xQueueSendFromISR(msgIcmInterruptQueueHandle, &raw, &px_higher_priority_task_woken);
	if (res != pdTRUE) {

	}
  }
#endif
#if IMU_DEVICE == IMU_ADIS16460 || IMU_DEVICE == IMU_KYIMU102
  if (GPIO_Pin == ADI_INT_Pin) { /** ADI 惯导数据ready */
	if (flag_timing != TIMING_OK) {
	  return;
	}
/*	local_time += 1;*/
	ImuMsg raw;
//	trace(Info,"imu ready\n");
	BaseType_t res, px_higher_priority_task_woken;
	/** tims_s = gpst0 + (1e6*timer_cnt + cnt)/pps_margin
	 * */
	raw.data.id = 4;
//	trace(Info, "imt int time :%lu %lu %lu\n",(int)system_time_start, system_time_int,htim2.Instance->CNT);
	raw.gpst = system_time_start + (system_time_int) + htim2.Instance->CNT * 1.0 / (double)pps_margin;/**/
	res = xQueueSendFromISR(msgIcmInterruptQueueHandle, &raw, &px_higher_priority_task_woken);
  }
#endif
/*#if IMU_DEVICE== IMU_KYIMU102
  if(GPIO_Pin==ADI_INT_Pin){

  }
#endif*/
#if GNSS_RCV == GNSS_RCV_UB482
  if (GPIO_Pin == GNSS_PPS_EXT_Pin) {/** GNSS PPS ready*/
#else
  if (GPIO_Pin == GNSS_PPS_Pin) {/** GNSS PPS ready*/
#endif
	if (flag_timing == WAIT_FOR_PPS) { /*本地时间校准*/
	  HAL_TIM_Base_Start_IT(&htim2);/*开启本地时间计时器*/
	  system_time_start = uart_gpst + 1.0;/*记录系统时间起点，本地计时器记录本地时间偏移，pps间隔记录二者比例，即两个pps之间有多少计数*/
	  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
	  system_time_int = 0;/*对时完成，本地时间清零*/
	  flag_timing = TIMING_OK;
	  trace(Info, "timing finished\n");
	}
	if (flag_timing != TIMING_OK) return;
	uint32_t cnt_current = htim2.Instance->CNT;
	/*pps_margin = (uint64_t)
		1000000LL * system_time_int + cnt_current - cnt_on_pps;*/
	/*for test*/
	cnt_on_pps = 1000000LL * system_time_int + cnt_current;

/*	if (pps_margin > 1000000 + 10 || pps_margin < 1000000 - 10) { // 避免pps中断造成margin计算异常
	  pps_margin = 1e6;
	}*/
//	htim2.Instance->ARR = pps_margin - 1;
  }
}
//;
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t rx_fifo_0_i_ts) {
  uint8_t can1RxData[8];
  VelocityMsg rawvel;
  if (hfdcan->Instance == FDCAN1) {
	BaseType_t res, px_higher_priority_task_woken;
	HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &RxHeader, (uint8_t *)&rawvel.data);
	if (RxHeader.Identifier == AGILEX_VELOCITY_IDENTIFIER) {
	  /*速度判定*/
//	  agilexGetVelocity(&vel, &RxHeader, can1RxData);\
/*TODO 校验位忽略了*/
/*	  if(!agilex_check(RxHeader.Identifier,(uint8_t *)&vel,8)){
	  return -1;
	  }*/
//	  ConvertVelRawToFloat(&rawvel, &vel);
	  rawvel.gpst = system_time_start + (system_time_int) + htim2.Instance->CNT * 1.0 / pps_margin;/**/
	  res = xQueueSendFromISR(velQueueHandle, &rawvel, &px_higher_priority_task_woken);
	  if (res != pdTRUE) {
		/*意味着速度不够*/
	  }
	}
  }
  if ((rx_fifo_0_i_ts & FDCAN_IT_RX_FIFO0_WATERMARK) != RESET) {
  }
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
  printf("data receive\n");
}
